#include "ms575_algbacks4s5s6ldmeasure.h"
using namespace MeasurE575;

AlgBackS4S5S6LDmeasure::AlgBackS4S5S6LDmeasure(int i):AlgBase(i)
{

}
void AlgBackS4S5S6LDmeasure::operator()(cv::Mat &srcImg,
                                        int id,
                                        std::map<string, float> &measRes,
                                        int fixtureId)
{
//back  S4S5S6-> 29 29 30
    //int LDmeasure(cv::Mat srcimg, const vector<int> paras, vector<cv::Point2f>& respts);

    std::vector<int> paras5,paras6,paras7;
    for(int j=0;j< m_dataAllVec[29].m_edgeParam.cols;j++)
    {
        paras5.push_back(m_dataAllVec[29].m_edgeParam.at<int>(5,j));
    }
    for(int j=0;j< m_dataAllVec[29].m_edgeParam.cols;j++)
    {
        paras6.push_back(m_dataAllVec[29].m_edgeParam.at<int>(6,j));
    }
    for(int j=0;j< m_dataAllVec[30].m_edgeParam.cols;j++)
    {
        paras7.push_back(m_dataAllVec[30].m_edgeParam.at<int>(7,j));
    }
    std::vector<cv::Point2f> resultS3,resultS4,resultS5;
    std::vector<cv::Rect> rangS3Vec,rangS4Vec,rangS5Vec;

    {
        TemplateMatch(srcImg,m_dataAllVec[28],rangS3Vec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangS3Vec[0]), paras5, resultS3);
        for(auto &ci:resultS3)
        {
            ci.x+=rangS3Vec[0].x;
            ci.y+=rangS3Vec[0].y;
        }

    }

    {
        TemplateMatch(srcImg,m_dataAllVec[29],rangS4Vec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangS4Vec[0]), paras6, resultS4);
        for(auto &ci:resultS4)
        {
            ci.x+=rangS4Vec[0].x;
            ci.y+=rangS4Vec[0].y;
        }
    }
    {
        TemplateMatch(srcImg,m_dataAllVec[30],rangS5Vec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangS5Vec[0]), paras7, resultS5);
        for(auto &ci:resultS5)
        {
            ci.x+=rangS5Vec[0].x;
            ci.y+=rangS5Vec[0].y;
        }
    }
    std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
    while (!AlgBase::flagVec[id])
    {
        (*AlgBase::cvarptrVec[id]).wait(lock);
    }
#pragma omp parallel sections
    {
        #pragma omp section
        {

            for(auto &pt2:resultS3)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }

         }
        #pragma omp section
          {

            for(auto &pt2:resultS4)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
          }
        #pragma omp section
        {

            for(auto &pt2:resultS5)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
        }
     }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    edgedetect obj;
    float lineb=obj.GetParallelism(resultS5,2);
    measRes.insert(std::make_pair(string("Line-B-parallelism_pixel"), lineb));
    float linebr = CalcMeasureDistance(m_phypixpara[prefix + std::string("Line-B-parallelism")], lineb);
    measRes.insert(std::make_pair(string("Line-B-parallelism_real"), linebr));

    float linec=obj.GetParallelism(resultS4,1);
    measRes.insert(std::make_pair(string("Line-C-parallelism_pixel"), linec));
    float linecr = CalcMeasureDistance(m_phypixpara[prefix + std::string("Line-C-parallelism")], linec);
    measRes.insert(std::make_pair(string("Line-C-parallelism_real"), linecr));

    float lined=obj.GetParallelism(resultS3,2);
    measRes.insert(std::make_pair(string("Line-D-parallelism_pixel"), lined));
    float linedr = CalcMeasureDistance(m_phypixpara[prefix + std::string("Line-D-parallelism")], lined);
    measRes.insert(std::make_pair(string("Line-D-parallelism_real"), linedr));
}
